System and method for control of an autonomous vehicle

ABSTRACT

An automotive vehicle includes an actuator configured to control vehicle steering, a sensor configured to detect a yaw rate of the vehicle, and a controller. The controller is configured to estimate a yaw rate and lateral velocity of the vehicle via a vehicle dynamics model based on a measured longitudinal velocity of the vehicle, calculated road wheel angles of the vehicle, and estimated tire slip angles of the vehicle. The controller is configured to receive a measured yaw rate from the sensor, and to calculate a difference between the measured yaw rate and the estimated yaw rate. The controller is configured to apply a model correction to the vehicle dynamics model using a PID controller based on the difference, and to estimate a vehicle position based on the estimated lateral velocity and the measured longitudinal velocity. The controller is configured to automatically control the actuator based on the vehicle position.

INTRODUCTION

The present disclosure relates to vehicles having sensor suites, e.g.those controlled by automated driving systems configured toautomatically control vehicle steering, acceleration, and braking duringa drive cycle without human intervention.

The operation of modern vehicles is becoming more automated, i.e. ableto provide driving control with less and less driver intervention.Vehicle automation has been categorized into numerical levels rangingfrom Zero, corresponding to no automation with full human control, toFive, corresponding to full automation with no human control. Variousautomated driver-assistance systems, such as cruise control, adaptivecruise control, and parking assistance systems correspond to lowerautomation levels, while true “driverless” vehicles correspond to higherautomation levels. As automation levels increase, sensor precision andconfidence requirements likewise increase.

SUMMARY

An automotive vehicle according to the present disclosure includes anactuator configured to control vehicle steering, a sensor configured todetect a yaw rate of the vehicle, and a controller. The controller isconfigured to estimate a yaw rate and lateral velocity of the vehiclevia a vehicle dynamics model based on a measured longitudinal velocityof the vehicle, calculated road wheel angles of the vehicle, andestimated tire slip angles of the vehicle. The controller is alsoconfigured to receive a measured yaw rate from the sensor, and tocalculate a difference between the measured yaw rate and the estimatedyaw rate. The controller is additionally configured to apply a modelcorrection to the vehicle dynamics model using a PID controller based onthe difference, and to estimate a vehicle position based on theestimated lateral velocity and the measured longitudinal velocity. Thecontroller is further configured to automatically control the actuatorbased on the vehicle position.

In an exemplary embodiment, the vehicle additionally includes a GNSSreceiver configured to provide a position of the vehicle. In suchembodiments, the controller is configured to estimate the vehicleposition based on the estimated lateral velocity and the measuredlongitudinal velocity in further response to a position from the GNSSreceiver being unavailable.

In an exemplary embodiment, the PID controller is a PI controller.

A method of controlling a ground vehicle according to the presentdisclosure includes providing the vehicle with an actuator configured tocontrol vehicle steering, at least one sensor configured to detect a yawrate of the vehicle and a longitudinal velocity of the vehicle, and acontroller in communication with the sensor and the controller. Themethod additionally includes receiving, via the at least one sensor, ameasured longitudinal velocity and a measured yaw rate. The method alsoincludes estimating, via the controller, a yaw rate and lateral velocityof the vehicle using a vehicle dynamics model based on the measuredlongitudinal velocity, calculated road wheel angles of the vehicle, andestimated tire slip angles of the vehicle. The method further includescalculating, via the controller, a difference between the measured yawrate and the estimated yaw rate, and applying, via the controller, amodel correction to the vehicle dynamics model using a PID controllerbased on the difference. The method further includes estimating, via thecontroller, a vehicle position based on the estimated lateral velocityand the measured longitudinal velocity, and automatically controllingthe actuator, via the controller, based on the vehicle position.

In an exemplary embodiment, the method additionally includes providingthe vehicle with a GNSS receiver configured to provide a position of thevehicle. In such embodiments, estimating the vehicle position based onthe estimated lateral velocity and the measured longitudinal velocity isin response to a position from the GNSS receiver being unavailable.

In an exemplary embodiment, the PID controller is a PI controller.

In an exemplary embodiment, the method additionally includes identifyingtuning constants of the PID controller via machine learning.

A control system for a vehicle according to the present disclosureincludes a first calculation module, a second calculation module, avehicle dynamics model, a path planning module, and a vehicle controlmodule. The first calculation module is configured to receive a measuredsteering angle and determine a calculated road wheel angle based on themeasured steering angle. The second calculation module is configured todetermine a calculated tire slip angle based on a measured longitudinalvelocity, a measured yaw rate, and a calculated lateral velocity. Thevehicle dynamics model is configured to determine a calculated yaw rate,the calculated lateral velocity, and a calculated position of thevehicle based on the measured longitudinal velocity of the vehicle, thecalculated road wheel angle, and the calculated tire slip angle. The PIDcontroller is configured to apply a model correction to the vehicledynamics model based on a difference between the measured yaw rate andthe calculated yaw rate. The path planning module is configured todetermine a vehicle path to be followed based on the calculated positionof the vehicle. The vehicle control module is configured toautomatically control a steering actuator based on the vehicle path.

In an exemplary embodiment, the control system additionally includes avehicle steering system. In such embodiment, the steering actuator isoperable to automatically control the vehicle steering system withoutoperator input.

In an exemplary embodiment, the control system additionally includes aGNSS receiver configured to provide a position of the vehicle. In suchembodiments, the path planning module is configured to determine avehicle path to be followed based on the calculated position of thevehicle in response to a position from the GNSS receiver beingunavailable.

In an exemplary embodiment, the PID controller is a PI controller.

In an exemplary embodiment, the control system additionally includes acoordinate alignment module configured to convert the calculatedposition of the vehicle from a first coordinate system to a secondcoordinate system.

Embodiments according to the present disclosure provide a number ofadvantages. For example, the present disclosure provides a system andmethod for calculating vehicle position using an accurate andcomputationally-efficient control. Moreover, the present disclosureprovides a method for automatically using such a control when desirablebased on the condition of one or more primary positioning sensors,thereby increasing robustness and reliability of the system and, inturn, increasing customer satisfaction.

The above and other advantages and features of the present disclosurewill be apparent from the following detailed description of thepreferred embodiments when taken in connection with the accompanyingdrawings.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a schematic diagram of a communication system including anautonomously controlled vehicle according to an embodiment of thepresent disclosure;

FIG. 2 is a schematic block diagram of an automated driving system (ADS)for a vehicle according to an embodiment of the present disclosure;

FIG. 3 is a block diagram representing a control scheme for a vehicleaccording to an embodiment of the present disclosure; and

FIG. 4 is a flowchart representation of a method of controlling avehicle according to an embodiment of the present disclosure.

DETAILED DESCRIPTION

Embodiments of the present disclosure are described herein. It is to beunderstood, however, that the disclosed embodiments are merely examplesand other embodiments can take various and alternative forms. Thefigures are not necessarily to scale; some features could be exaggeratedor minimized to show details of particular components. Therefore,specific structural and functional details disclosed herein are not tobe interpreted as limiting, but are merely representative. The variousfeatures illustrated and described with reference to any one of thefigures can be combined with features illustrated in one or more otherfigures to produce embodiments that are not explicitly illustrated ordescribed. The combinations of features illustrated providerepresentative embodiments for typical applications. Variouscombinations and modifications of the features consistent with theteachings of this disclosure, however, could be desired for particularapplications or implementations.

FIG. 1 schematically illustrates an operating environment that comprisesa mobile vehicle communication and control system 10 for a motor vehicle12. The motor vehicle 12 may be referred to as a host vehicle. Thecommunication and control system 10 for the host vehicle 12 generallyincludes one or more wireless carrier systems 60, a land communicationsnetwork 62, a computer 64, a mobile device 57 such as a smart phone, anda remote access center 78.

The host vehicle 12, shown schematically in FIG. 1, is depicted in theillustrated embodiment as a passenger car, but it should be appreciatedthat any other vehicle including motorcycles, trucks, sport utilityvehicles (SUVs), recreational vehicles (RVs), marine vessels, aircraft,etc., can also be used. The host vehicle has a body with a fore-aftlongitudinal axis and a side-to-side lateral axis.

The host vehicle 12 includes a propulsion system 13, which may invarious embodiments include an internal combustion engine, an electricmachine such as a traction motor, and/or a fuel cell propulsion system.

The host vehicle 12 also includes a transmission 14 configured totransmit power from the propulsion system 13 to a plurality of vehiclewheels 15 according to selectable speed ratios. According to variousembodiments, the transmission 14 may include a step-ratio automatictransmission, a continuously-variable transmission, or other appropriatetransmission. The host vehicle 12 additionally includes wheel brakes 17configured to provide braking torque to the vehicle wheels 15. The wheelbrakes 17 may, in various embodiments, include friction brakes, aregenerative braking system such as an electric machine, and/or otherappropriate braking systems.

The host vehicle 12 additionally includes a steering system 16. Whiledepicted as including a steering wheel for illustrative purposes, insome embodiments contemplated within the scope of the presentdisclosure, the steering system 16 may not include a steering wheel.

The host vehicle 12 includes a wireless communications system 28configured to wirelessly communicate with other vehicles (“V2V”) and/orinfrastructure (“V2I”). In an exemplary embodiment, the wirelesscommunication system 28 is configured to communicate via a dedicatedshort-range communications (DSRC) channel. DSRC channels refer toone-way or two-way short-range to medium-range wireless communicationchannels specifically designed for automotive use and a correspondingset of protocols and standards. However, wireless communications systemsconfigured to communicate via additional or alternate wirelesscommunications standards, such as IEEE 802.11 (“WiFi™”) and cellulardata communication, are also considered within the scope of the presentdisclosure.

The propulsion system 13, transmission 14, steering system 16, and wheelbrakes 17 are in communication with or under the control of at least onecontroller 22. While depicted as a single unit for illustrativepurposes, the controller 22 may additionally include one or more othercontrollers, collectively referred to as a “controller.” The controller22 may include a microprocessor or central processing unit (CPU) incommunication with various types of computer readable storage devices ormedia. Computer readable storage devices or media may include volatileand nonvolatile storage in read-only memory (ROM), random-access memory(RAM), and keep-alive memory (KAM), for example. KAM is a persistent ornon-volatile memory that may be used to store various operatingvariables while the CPU is powered down. Computer-readable storagedevices or media may be implemented using any of a number of knownmemory devices such as PROMs (programmable read-only memory), EPROMs(electrically PROM), EEPROMs (electrically erasable PROM), flash memory,or any other electric, magnetic, optical, or combination memory devicescapable of storing data, some of which represent executableinstructions, used by the controller 22 in controlling the vehicle.

The controller 22 includes an automated driving system (ADS) 24 forautomatically controlling various actuators in the vehicle. In anexemplary embodiment, the ADS 24 is a so-called Level Four or Level Fiveautomation system. A Level Four system indicates “high automation”,referring to the driving mode-specific (e.g. within defined geographicboundaries) performance by an automated driving system of all aspects ofthe dynamic driving task, even if a human driver does not respondappropriately to a request to intervene. A Level Five system indicates“full automation”, referring to the full-time performance by anautomated driving system of all aspects of the dynamic driving taskunder all roadway and environmental conditions that can be managed by ahuman driver.

Other embodiments according to the present disclosure may be implementedin conjunction with so-called Level One, Level Two, or Level Threeautomation systems. A Level One system indicates “driver assistance”,referring to the driving mode-specific execution by a driver assistancesystem of either steering or acceleration using information about thedriving environment and with the expectation that the human driverperforms all remaining aspects of the dynamic driving task. A Level Twosystem indicates “Partial Automation”, referring to the drivingmode-specific execution by one or more driver assistance systems of bothsteering and acceleration using information about the drivingenvironment and with the expectation that the human driver performs allremaining aspects of the dynamic driving task. A Level Three systemindicates “Conditional Automation”, referring to the drivingmode-specific performance by an automated driving system of all aspectsof the dynamic driving task with the expectation that the human driverwill respond appropriately to a request to intervene.

In an exemplary embodiment, the ADS 24 is configured to control thepropulsion system 13, transmission 14, steering system 16, and wheelbrakes 17 to control vehicle acceleration, steering, and braking,respectively, without human intervention via a plurality of actuators 30in response to inputs from a plurality of sensors 26, which may includeGNSS (global navigation satellite system, e.g. GPS and/or GLONASS),RADAR, LIDAR, optical cameras, thermal cameras, ultrasonic sensors,and/or additional sensors as appropriate.

FIG. 1 illustrates several networked devices that can communicate withthe wireless communication system 28 of the host vehicle 12. One of thenetworked devices that can communicate with the host vehicle 12 via thewireless communication system 28 is the mobile device 57. The mobiledevice 57 can include computer processing capability, a transceivercapable of communicating signals 58 using a short-range wirelessprotocol, and a visual smart phone display 59. The computer processingcapability includes a microprocessor in the form of a programmabledevice that includes one or more instructions stored in an internalmemory structure and applied to receive binary input to create binaryoutput. In some embodiments, the mobile device 57 includes a GPS modulecapable of receiving signals from GPS satellites 68 and generating GPScoordinates based on those signals. In other embodiments, the mobiledevice 57 includes cellular communications functionality such that themobile device 57 carries out voice and/or data communications over thewireless carrier system 60 using one or more cellular communicationsprotocols, as are discussed herein. The mobile device 57 may alsoinclude other sensors, including but not limited to, accelerometerscapable of measuring motion of the mobile device 57 along six axes. Thevisual smart phone display 59 may also include a touch-screen graphicaluser interface.

The wireless carrier system 60 is preferably a cellular telephone systemthat includes a plurality of cell towers 70 (only one shown), one ormore mobile switching centers (MSCs) 72, as well as any other networkingcomponents required to connect the wireless carrier system 60 with theland communications network 62. Each cell tower 70 includes sending andreceiving antennas and a base station, with the base stations fromdifferent cell towers being connected to the MSC 72 either directly orvia intermediary equipment such as a base station controller. Thewireless carrier system 60 can implement any suitable communicationstechnology, including for example, analog technologies such as AMPS, ordigital technologies such as CDMA (e.g., CDMA2000) or GSM/GPRS. Othercell tower/base station/MSC arrangements are possible and could be usedwith the wireless carrier system 60. For example, the base station andcell tower could be co-located at the same site or they could beremotely located from one another, each base station could beresponsible for a single cell tower or a single base station couldservice various cell towers, or various base stations could be coupledto a single MSC, to name but a few of the possible arrangements.

Apart from using the wireless carrier system 60, a second wirelesscarrier system in the form of satellite communication can be used toprovide unidirectional or bidirectional communication with the hostvehicle 12. This can be done using one or more communication satellites66 and an uplink transmitting station 67. Unidirectional communicationcan include, for example, satellite radio services, wherein programmingcontent (news, music, etc.) is received by the transmitting station 67,packaged for upload, and then sent to the satellite 66, which broadcaststhe programming to subscribers. Bidirectional communication can include,for example, satellite telephony services using the satellite 66 torelay telephone communications between the host vehicle 12 and thestation 67. The satellite telephony can be utilized either in additionto or in lieu of the wireless carrier system 60.

The land network 62 may be a conventional land-based telecommunicationsnetwork connected to one or more landline telephones and connects thewireless carrier system 60 to the remote access center 78. For example,the land network 62 may include a public switched telephone network(PSTN) such as that used to provide hardwired telephony, packet-switcheddata communications, and the Internet infrastructure. One or moresegments of the land network 62 could be implemented through the use ofa standard wired network, a fiber or other optical network, a cablenetwork, power lines, other wireless networks such as wireless localarea networks (WLANs), or networks providing broadband wireless access(BWA), or any combination thereof. Furthermore, the remote access center78 need not be connected via land network 62, but could include wirelesstelephony equipment so that it can communicate directly with a wirelessnetwork, such as the wireless carrier system 60.

While shown in FIG. 1 as a single device, the computer 64 may include anumber of computers accessible via a private or public network such asthe Internet. Each computer 64 can be used for one or more purposes. Inan exemplary embodiment, the computer 64 may be configured as a webserver accessible by the host vehicle 12 via the wireless communicationsystem 28 and the wireless carrier 60. Other computers 64 can include,for example: a service center computer where diagnostic information andother vehicle data can be uploaded from the vehicle via the wirelesscommunication system 28 or a third party repository to or from whichvehicle data or other information is provided, whether by communicatingwith the host vehicle 12, the remote access center 78, the mobile device57, or some combination of these. The computer 64 can maintain asearchable database and database management system that permits entry,removal, and modification of data as well as the receipt of requests tolocate data within the database. The computer 64 can also be used forproviding Internet connectivity such as DNS services or as a networkaddress server that uses DHCP or other suitable protocol to assign an IPaddress to the host vehicle 12. The computer 64 may be in communicationwith at least one supplemental vehicle in addition to the host vehicle12. The host vehicle 12 and any supplemental vehicles may becollectively referred to as a fleet. In an exemplary embodiment, thecomputer 64 is configured to store, e.g. in non-transient data memory,subscriber account information and/or vehicle information. Thesubscriber account information can include, but is not limited to,biometric data, password information, subscriber preferences, andlearned behavioral patterns of users or occupants of vehicles in thefleet. The vehicle information can include, but is not limited to,vehicle attributes such as color, make, model, license plate number,notification light pattern, and/or frequency identifiers.

As shown in FIG. 2, the ADS 24 includes multiple distinct systems,including at least a perception system 32 for determining the presence,location, classification, and path of detected features or objects inthe vicinity of the vehicle. The perception system 32 is configured toreceive inputs from a variety of sensors, such as the sensors 26illustrated in FIG. 1, and synthesize and process the sensor inputs togenerate parameters used as inputs for other control algorithms of theADS 24.

The perception system 32 includes a sensor fusion and preprocessingmodule 34 that processes and synthesizes sensor data 27 from the varietyof sensors 26. The sensor fusion and preprocessing module 34 performscalibration of the sensor data 27, including, but not limited to, LIDARto LIDAR calibration, camera to LIDAR calibration, LIDAR to chassiscalibration, and LIDAR beam intensity calibration. The sensor fusion andpreprocessing module 34 outputs preprocessed sensor output 35.

A classification and segmentation module 36 receives the preprocessedsensor output 35 and performs object classification, imageclassification, traffic light and sign classification, objectsegmentation, ground segmentation, and object tracking processes. Objectclassification includes, but is not limited to, identifying andclassifying objects in the surrounding environment includingidentification and classification of traffic signals and signs, RADARfusion and tracking to account for the sensor's placement and field ofview (FOV), and false positive rejection via LIDAR fusion to eliminatethe many false positives that exist in an urban environment, such as,for example, manhole covers, bridges, overhead trees or light poles, andother obstacles with a high RADAR cross section but which do not affectthe ability of the vehicle to travel along its path. Additional objectclassification and tracking processes performed by the classificationand segmentation module 36 include, but are not limited to, freespacedetection and high level tracking that fuses data from RADAR tracks,LIDAR segmentation, LIDAR classification, image classification, objectshape fit models, semantic information, motion prediction, raster maps,static obstacle maps, and other sources to produce high quality objecttracks. The classification and segmentation module 36 additionallyperforms traffic control device classification and traffic controldevice fusion with lane association and traffic control device behaviormodels. The classification and segmentation module 36 generates anobject classification and segmentation output 37 that includes objectidentification information.

A localization and mapping module 40 uses the object classification andsegmentation output 37 to calculate parameters including, but notlimited to, estimates of the position and orientation of the hostvehicle 12 in both typical and challenging driving scenarios. Thesechallenging driving scenarios include, but are not limited to, dynamicenvironments with many cars (e.g., dense traffic), environments withlarge scale obstructions (e.g., roadwork or construction sites), hills,multi-lane roads, single lane roads, a variety of road markings andbuildings or lack thereof (e.g., residential vs. business districts),and bridges and overpasses (both above and below a current road segmentof the vehicle).

The localization and mapping module 40 also incorporates new datacollected as a result of expanded map areas obtained via onboard mappingfunctions performed by the host vehicle 12 during operation and mappingdata “pushed” to the host vehicle 12 via the wireless communicationsystem 28. The localization and mapping module 40 updates previous mapdata with the new information (e.g., new lane markings, new buildingstructures, addition or removal of constructions zones, etc.) whileleaving unaffected map regions unmodified. Examples of map data that maybe generated or updated include, but are not limited to, yield linecategorization, lane boundary generation, lane connection,classification of minor and major roads, classification of left andright turns, and intersection lane creation. The localization andmapping module 40 generates a localization and mapping output 41 thatincludes the position and orientation of the host vehicle 12 withrespect to detected obstacles and road features.

A vehicle odometry module 46 receives data 27 from the vehicle sensors26 and generates a vehicle odometry output 47 which includes, forexample, vehicle heading and velocity information. An absolutepositioning module 42 receives the localization and mapping output 41and the vehicle odometry information 47 and generates a vehicle locationoutput 43 that is used in separate calculations as discussed below.

An object prediction module 38 uses the object classification andsegmentation output 37 to generate parameters including, but not limitedto, a location of a detected obstacle relative to the vehicle, apredicted path of the detected obstacle relative to the vehicle, and alocation and orientation of traffic lanes relative to the vehicle. Dataon the predicted path of objects (including pedestrians, surroundingvehicles, and other moving objects) is output as an object predictionoutput 39 and is used in separate calculations as discussed below.

The ADS 24 also includes an observation module 44 and an interpretationmodule 48. The observation module 44 generates an observation output 45received by the interpretation module 48. The observation module 44 andthe interpretation module 48 allow access by the remote access center78. The interpretation module 48 generates an interpreted output 49 thatincludes additional input provided by the remote access center 78, ifany.

A path planning module 50 processes and synthesizes the objectprediction output 39, the interpreted output 49, and additional routinginformation 79 received from an online database or the remote accesscenter 78 to determine a vehicle path to be followed to maintain thevehicle on the desired route while obeying traffic laws and avoiding anydetected obstacles. The path planning module 50 employs algorithmsconfigured to avoid any detected obstacles in the vicinity of thevehicle, maintain the vehicle in a current traffic lane, and maintainthe vehicle on the desired route. The path planning module 50 outputsthe vehicle path information as path planning output 51. The pathplanning output 51 includes a commanded vehicle path based on thevehicle route, vehicle location relative to the route, location andorientation of traffic lanes, and the presence and path of any detectedobstacles.

A first control module 52 processes and synthesizes the path planningoutput 51 and the vehicle location output 43 to generate a first controloutput 53. The first control module 52 also incorporates the routinginformation 79 provided by the remote access center 78 in the case of aremote take-over mode of operation of the vehicle.

A vehicle control module 54 receives the first control output 53 as wellas velocity and heading information 47 received from vehicle odometry 46and generates vehicle control output 55. The vehicle control output 55includes a set of actuator commands to achieve the commanded path fromthe vehicle control module 54, including, but not limited to, a steeringcommand, a shift command, a throttle command, and a brake command.

The vehicle control output 55 is communicated to actuators 30. In anexemplary embodiment, the actuators 30 include a steering control, ashifter control, a throttle control, and a brake control. The steeringcontrol may, for example, control a steering system 16 as illustrated inFIG. 1. The shifter control may, for example, control a transmission 14as illustrated in FIG. 1. The throttle control may, for example, controla propulsion system 13 as illustrated in FIG. 1. The brake control may,for example, control wheel brakes 17 as illustrated in FIG. 1.

Robustness of sensor data is important in all vehicles, but particularlywhen the vehicle is under the control of the ADS 24, and thereby relyingheavily upon sensor data for satisfactory operation. When the vehicle isunder the control of the ADS 24 it is desirable to know the precisevehicle location relative to a desired path to enable accuratenavigation and path-following behavior. However, in some drivingsituations such as urban canyons (e.g. when in the vicinity of tallobstructions such as buildings), obstructions may result in impairedability to accurately determine the position of the vehicle 12 usingGNSS location. Likewise, some driving situations. such as inclementweather, may impair the ability of vehicle cameras to accuratelydetermine the position of the vehicle relative to lane markings or otherindicators of desired vehicle path. It is thereby desirable to provide arobust backup positioning system, to enable accurate vehicle positioningwhen GNSS is unavailable.

Referring now to FIG. 3, a method of estimating vehicle positionaccording to the present disclosure is illustrated in block diagramform. In an exemplary embodiment, the method is performed by thecontroller 22, e.g. as part of the localization and mapping module 40and/or the positioning module 42. Any specific equations discussed beloware merely exemplary and are not intended to limit the scope of thepresent disclosure.

Vehicle inputs 80 are received, e.g. via one or more sensors 26. In anexemplary embodiment, the vehicle inputs 80 include longitudinalvelocity of the vehicle 12, yaw rate of the vehicle 12, and a steeringangle of the steering system 16.

Ackermann steering parameters 82 are then calculated. The Ackermannsteering parameters include a left road wheel angle and a right roadwheel angle. The Ackermann steering parameters are calculated based onthe vehicle inputs 80, in conjunction with vehicle-specific parameters.The vehicle-specific parameters refer to parameters which are generallyconstant for a given vehicle model, including (but not limited to) frontand/or rear cornering coefficient(s), moment of inertia, massdistribution, track width and wheelbase, and steering geometry such astoe angle of the steering system 16. In an exemplary embodiment, thiscalculation may be expressed as:

${steering}\mspace{14mu} {ratio}{= {A_{0} + {A_{1}\theta_{SW}} + {A_{2}\theta_{SW}^{2}} + {B_{0}{\cos ( \frac{2*( {\theta_{SW} - C} )}{5{7.3}} )}}}}$

where θ_(SW) refers to a steering wheel angle, and A₀, A₁, A₂, B₀, and Care constants based on the above-described parameters.

A tire slip angle calculation 84 is thereafter performed based on thevehicle inputs 80 and the above-described vehicle-specific parameters.In an exemplary embodiment, the tire slip angle calculation estimatestire slip angles for all vehicle wheels based on inputs including thelongitudinal velocity, lateral velocity, and yaw rate. In an exemplaryembodiment, the tire slip angle calculation may be expressed as:

${\alpha_{L\; 1} = {\delta_{f} - {a\; {\tan \lbrack \frac{v_{y} + {L_{f}r}}{u - {0.5\; {rw}}} \rbrack}} + T_{L\; 1}}};$${\alpha_{R\; 1} = {\delta_{f} - {a\; {\tan \lbrack \frac{v_{y} + {L_{f}r}}{u + {0.5\; {rw}}} \rbrack}} + T_{R\; 1}}};$${\alpha_{L\; 2} = {\delta_{r} - {a\; {\tan \lbrack \frac{v_{y} + {L_{r}r}}{u - {0.5\; {rw}}} \rbrack}} + T_{L\; 2}}};$${\alpha_{R\; 2} = {\delta_{r} - {a\; {\tan \lbrack \frac{v_{y} + {L_{r}r}}{u + {0.5\; {rw}}} \rbrack}} + T_{R\; 2}}};$${\alpha_{f} = {\frac{1}{2}( {\alpha_{L\; 1} + \alpha_{R\; 1}} )}};$${\alpha_{r} = {\frac{1}{2}( {\alpha_{L\; 2} + \alpha_{R\; 2}} )}};$F_(yf) = c_(α_(f))α_(f); F_(y r) = c_(α_(r))α_(r)

where α_(L1), α_(R1), α_(L2), and α_(R2) are slip angles of front left,front right, rear left, and rear right tires, respectively; δ_(f) andδ_(r) are front and rear road wheel angles, respectively; vγ is thelateral velocity of the vehicle; L_(f) and L_(r) are the distance fromthe vehicle center of gravity to the center of front and rear axles,respectively; r is yaw rate of the vehicle; u is the longitudinalvelocity of the vehicle; w is the track width of the vehicle; T_(L1),T_(R1), T_(L2), and T_(R2) are toe angles of front left, front right,rear left, and rear right tires, respectively; α_(f) and α_(r) areaverage front and rear slip angles, respectively; F_(yf) and F_(yr) arelateral forces at front and rear axles, respectively; and c_(α) _(f) andc_(α) _(r) are cornering stiffness parameters for front and rear axles,respectively.

A state estimator 86 thereafter estimates vehicle state variables. In anexemplary embodiment, the state variables include longitudinal position,lateral position, yaw rate, and lateral velocity. The state estimatormay be based on a so-called “bicycle model” of vehicle kinematics, orany other suitable model for estimating such parameters. In an exemplaryembodiment, the state estimator may be expressed as:

$\begin{bmatrix}{dP_{y}} \\{dP_{r}} \\{dL_{x}} \\{dL_{y}}\end{bmatrix} = {\begin{bmatrix}0 & {- ( \frac{mU}{I_{ZZ}} )} & 0 & 0 \\0 & 0 & 0 & 0 \\{- ( \frac{\sin \; \theta_{v}}{m} )} & 0 & 0 & 0 \\( \frac{\cos \; \theta_{v}}{m} ) & 0 & 0 & 0\end{bmatrix}{\quad{\begin{bmatrix}P_{y} \\P_{r} \\L_{x} \\L_{y}\end{bmatrix} + {\begin{bmatrix}{\cos \mspace{11mu} \delta} & 1 \\{L_{f}\cos \; \delta} & {- L_{r}} \\0 & 0 \\0 & 0\end{bmatrix}\begin{bmatrix}F_{yf} \\F_{yr}\end{bmatrix}} + \begin{bmatrix}0 \\0 \\{u\; \cos \; \theta_{v}} \\{\; {u\; \sin \; \theta_{v}}}\end{bmatrix}}}}$

where P_(y) and P_(r) are lateral linear momentum and yaw angularmomentum of the vehicle, respectively; L_(x) and L_(y) are x- andy-coordinates of the vehicle, respectively; θ_(v) is the yaw angle ofthe vehicle; m is vehicle mass; I_(ZZ) is the moment of inertia of thevehicle; δ is an average road wheel angle of the vehicle, e.g. anaverage of δ_(f) and δ_(r); and u, F_(yf), F_(yr), L_(f), and L_(r) areas discussed above.

A PID controller 88 is implemented to minimize error in the stateestimator 86. The PID controller utilizes, as its input, a differencebetween the estimated yaw rate output by the state estimator and themeasured yaw rate from the input 80. The tuning constants of the P, I,and D terms may be obtained by any appropriate tuning technique,including (but not limited to) machine learning. In an exemplaryembodiment, the tuning constant of the D term is 0, resulting in a PIcontrol scheme. The output of the PID controller 88 is provided to thestate estimator 86 as a correction factor, increasing precision of theestimated states.

A coordinate alignment calculation 90 is optionally thereafter performedto convert vehicle position into an appropriate coordinate system. Sucha calculation may be used when, for example, the state estimator 86calculates a vehicle location in a vehicle-centric coordinate systemwhile path planning is performed in a global coordinate system. However,in some embodiments a consistent coordinate system may be usedthroughout the ADS 24, obviating the coordinate alignment calculation90.

A calculated vehicle location 92 is thereafter output. As will bediscussed in further detail below, the calculated vehicle location maybe used for path planning purposes to more precisely navigate thevehicle 12 according to the desired route.

Referring now to FIG. 4, a method of controlling a vehicle according toan embodiment of the present disclosure is illustrated in flowchartform. The method may be performed by the controller 22, e.g. as part ofthe ADS 24. Aspects of the method may be embodied in the localizationand mapping module 40 and/or the positioning module 42. The algorithmbegins at block 100.

A determination is made of whether the vehicle is under ADS control, asillustrated at operation 102. This determination may be satisfied inresponse to one or more aspects of vehicle driving, e.g. vehiclesteering, being controlled automatically by the ADS.

In response to the determination of operation 102 being negative, e.g.when the vehicle is under manual control of an occupant of the vehicleor under the control of a remote operator, control returns to operation102. The algorithm thereby does not proceed unless and until the vehicleis under ADS control.

A determination is then made of whether one or more primary positionsensors are available, as illustrated at operation 104. Thisdetermination may be satisfied when all sensors 26 relied upon by theprimary positioning algorithm of the ADS 24, e.g. a GNSS sensor, cameramodule, RADAR module, LiDAR module, or any combination thereof, areavailable and providing satisfactory data.

In response to the determination of operation 104 being positive, i.e.all of the sensors 26 are available, then a primary positioning outputis used, as illustrated at block 106. This may be, e.g., the positioningoutput of a default positioning algorithm of the positioning module 42.

The vehicle is subsequently automatically controlled to achieve adesired path based on the positioning output, as illustrated at block108. This may comprise automatically controlling one or more of theactuators 30 to achieve the desired path, e.g. the output from the pathplanning module. Control then returns to operation 102.

Returning to operation 104, in response to the determination beingnegative, i.e. one or more of the primary position sensors beingunavailable, then real-time inputs are received, as illustrated at block110. In an exemplary embodiment, these are generally the same inputs asdiscussed with respect to FIG. 3, e.g. the inputs 80.

A vehicle state is estimated, as illustrated at block 112. In anexemplary embodiment, this is generally the same estimation as discussedwith respect to FIG. 3, e.g. the state estimation 86.

The vehicle state is then refined based on a PID controller, asillustrated at block 114. In an exemplary embodiment, this controller isarranged generally as discussed with respect to FIG. 3, e.g. as the PIDcontroller 88.

A secondary positioning output is obtained, as illustrated at block 116.In an exemplary embodiment, the secondary positioning output is obtainedgenerally as discussed with respect to FIG. 4, e.g. as with the output92.

Control then proceeds to block 108 and the vehicle is automaticallycontrolled to achieve the desired path based on the positioning output.The algorithm thereafter returns to operation 102.

As may be seen, the algorithm thereby obtains a position via the backuppositioning algorithm discussed herein whenever one or more primaryposition sensor is unavailable.

As may be seen, the present disclosure provides a system and method forcalculating vehicle position using an accurate andcomputationally-efficient control. Moreover, the present disclosureprovides a method for automatically using such a control when desirablebased on the condition of one or more primary positioning sensors,thereby increasing robustness and reliability of the system and, inturn, increasing customer satisfaction.

While exemplary embodiments are described above, it is not intended thatthese embodiments describe all possible forms encompassed by the claims.The words used in the specification are words of description rather thanlimitation, and it is understood that various changes can be madewithout departing from the spirit and scope of the disclosure. Aspreviously described, the features of various embodiments can becombined to form further exemplary aspects of the present disclosurethat may not be explicitly described or illustrated. While variousembodiments could have been described as providing advantages or beingpreferred over other embodiments or prior art implementations withrespect to one or more desired characteristics, those of ordinary skillin the art recognize that one or more features or characteristics can becompromised to achieve desired overall system attributes, which dependon the specific application and implementation. These attributes caninclude, but are not limited to cost, strength, durability, life cyclecost, marketability, appearance, packaging, size, serviceability,weight, manufacturability, ease of assembly, etc. As such, embodimentsdescribed as less desirable than other embodiments or prior artimplementations with respect to one or more characteristics are notoutside the scope of the disclosure and can be desirable for particularapplications.

What is claimed is:
 1. A ground vehicle comprising: an actuatorconfigured to control vehicle steering; a sensor configured to detect ayaw rate of the vehicle; and a controller configured to estimate a yawrate and lateral velocity of the vehicle via a vehicle dynamics modelbased on a measured longitudinal velocity of the vehicle, calculatedroad wheel angles of the vehicle, and estimated tire slip angles of thevehicle, to receive a measured yaw rate from the sensor, to calculate adifference between the measured yaw rate and the estimated yaw rate, toapply a model correction to the vehicle dynamics model using a PIDcontroller based on the difference, to estimate a vehicle position basedon the estimated lateral velocity and the measured longitudinalvelocity, and to automatically control the actuator based on the vehicleposition.
 2. The vehicle of claim 1, further comprising a GNSS receiverconfigured to provide a position of the vehicle, wherein the controlleris configured to estimate the vehicle position based on the estimatedlateral velocity and the measured longitudinal velocity in response to aposition from the GNSS receiver being unavailable.
 3. The vehicle ofclaim 1, wherein the PID controller is a PI controller.
 4. A method ofcontrolling a ground vehicle, comprising: providing the vehicle with anactuator configured to control vehicle steering, at least one sensorconfigured to detect a yaw rate of the vehicle and a longitudinalvelocity of the vehicle, and a controller in communication with thesensor and the controller; receiving, via the at least one sensor, ameasured longitudinal velocity and a measured yaw rate; estimating, viathe controller, a yaw rate and lateral velocity of the vehicle using avehicle dynamics model based on the measured longitudinal velocity,calculated road wheel angles of the vehicle, and estimated tire slipangles of the vehicle; calculating, via the controller, a differencebetween the measured yaw rate and the estimated yaw rate; applying, viathe controller, a model correction to the vehicle dynamics model using aPID controller based on the difference; estimating, via the controller,a vehicle position based on the estimated lateral velocity and themeasured longitudinal velocity; and automatically controlling theactuator, via the controller, based on the vehicle position.
 5. Themethod of claim 4, further comprising providing the vehicle with a GNSSreceiver configured to provide a position of the vehicle, wherein theestimating the vehicle position based on the estimated lateral velocityand the measured longitudinal velocity is in response to a position fromthe GNSS receiver being unavailable.
 6. The method of claim 4, whereinthe PID controller is a PI controller.
 7. The method of claim 4, furthercomprising identifying tuning constants of the PID controller viamachine learning.
 8. A control system for a vehicle, the control systemcomprising: a first calculation module configured to receive a measuredsteering angle and determine a calculated road wheel angle based on themeasured steering angle; a second calculation module configured todetermine a calculated tire slip angle based on a measured longitudinalvelocity, a measured yaw rate, and a calculated lateral velocity; avehicle dynamics model configured to determine a calculated yaw rate,the calculated lateral velocity, and a calculated position of thevehicle based on the measured longitudinal velocity of the vehicle, thecalculated road wheel angle, and the calculated tire slip angle; a PIDcontroller configured to apply a model correction to the vehicledynamics model based on a difference between the measured yaw rate andthe calculated yaw rate; a path planning module configured to determinea vehicle path to be followed based on the calculated position of thevehicle; and a vehicle control module configured to automaticallycontrol a steering actuator based on the vehicle path.
 9. The controlsystem of claim 8, further comprising a vehicle steering system, whereinthe steering actuator is operable to automatically control the vehiclesteering system without operator input.
 10. The control system of claim8, further comprising a GNSS receiver configured to provide a positionof the vehicle, wherein the path planning module configured to determinea vehicle path to be followed based on the calculated position of thevehicle in response to a position from the GNSS receiver beingunavailable.
 11. The control system of claim 8, wherein the PIDcontroller is a PI controller.
 12. The control system of claim 8,further comprising a coordinate alignment module configured to convertthe calculated position of the vehicle from a first coordinate system toa second coordinate system.